/********************************************************************
 *
 *      Filename: VectorMath.cpp
 *   Description: See VectorMath.h
 *        Author: [kb]ypp
 *    Created at: December 13, 2006
 *
 ********************************************************************/

#include "MathUtil.h"
#include "VectorMath.h"
#include "../core/Log.h"

namespace math {

//! @brief Assignment with addition
template< typename T >
Vec2< T > operator+=( Vec2< T > lhs, Vec2< T > rhs )
{
    return ( lhs = lhs + rhs );
}

template< typename T >
Vec3< T > operator+=( Vec3< T > lhs, Vec3< T > rhs )
{
    return ( lhs = lhs + rhs );
}

template< typename T >
Vec4< T > operator+=( Vec4< T > lhs, Vec4< T > rhs )
{
    return ( lhs = lhs + rhs );
}

//! @brief Assignment with subtraction
template< typename T >
Vec2< T > operator-=( Vec2< T > lhs, Vec2< T > rhs )
{
    return ( lhs = lhs - rhs );
}

template< typename T >
Vec3< T > operator-=( Vec3< T > lhs, Vec3< T > rhs )
{
    return ( lhs = lhs - rhs );
}

//! @brief Assignment with multiplication to scalar
template< typename T >
Vec4< T > operator-=( Vec4< T > lhs, Vec4< T > rhs )
{
    return ( lhs = lhs - rhs );
}

//! @brief Assignment with multiplication to scalar
template< typename T >
Vec2< T > operator*=( Vec2< T > lhs, typename Vec2< T >::coord_type factor )
{
    return ( lhs = lhs * factor );
}

template< typename T >
Vec3< T > operator*=( Vec3< T > lhs, typename Vec3< T >::coord_type factor )
{
    return ( lhs = lhs * factor );
}

template< typename T >
Vec4< T > operator*=( Vec4< T > lhs, typename Vec4< T >::coord_type factor )
{
    return ( lhs = lhs * factor );
}

//! @brief Positive of a vector (unary '+')
template< typename T >
Vec2< T > operator+( Vec2< T > v )
{
    return v;
}

template< typename T >
Vec3< T > operator+( Vec3< T > v )
{
    return v;
}

template< typename T >
Vec4< T > operator+( Vec4< T > v )
{
    return v;
}

//! @brief Negation of a vector (unary '-')
template< typename T >
Vec2< T > operator-( Vec2< T > v )
{
    return Vec2< T >( -v.x, -v.y );
}

template< typename T >
Vec3< T > operator-( Vec3< T > v )
{
    return Vec3< T >( -v.x, -v.y, -v.z );
}

template< typename T >
Vec4< T > operator-( Vec4< T > v )
{
    return Vec4< T >( -v.x, -v.y, -v.z, -v.w );
}

//! @brief Componentwise vector addition
template< typename T >
Vec2< T > operator+( Vec2< T > lhs, Vec2< T > rhs )
{
    return Vec2< T >(
        lhs.x + rhs.x,
        lhs.y + rhs.y );
}

template< typename T >
Vec3< T > operator+( Vec3< T > lhs, Vec3< T > rhs )
{
    return Vec3< T >(
        lhs.x + rhs.x,
        lhs.y + rhs.y,
        lhs.z + rhs.z );
}

template< typename T >
Vec4< T > operator+( Vec4< T > lhs, Vec4< T > rhs )
{
    return Vec3< T >(
        lhs.x + rhs.x,
        lhs.y + rhs.y,
        lhs.z + rhs.z,
        lhs.w + rhs.w );
}

//! @brief Componentwise vector subtraction
template< typename T >
Vec2< T > operator-( Vec2< T > lhs, Vec2< T > rhs )
{
    return Vec2< T >(
        lhs.x - rhs.x,
        lhs.y - rhs.y );
}

template< typename T >
Vec3< T > operator-( Vec3< T > lhs, Vec3< T > rhs )
{
    return Vec3< T >(
        lhs.x - rhs.x,
        lhs.y - rhs.y,
        lhs.z - rhs.z );
}

template< typename T >
Vec4< T > operator-( Vec4< T > lhs, Vec4< T > rhs )
{
    return Vec3< T >(
        lhs.x - rhs.x,
        lhs.y - rhs.y,
        lhs.z - rhs.z,
        lhs.w - rhs.w );
}

//! @brief The product of scalar and a vector
template< typename T >
Vec2< T > operator*( typename Vec2< T >::coord_type factor, Vec2< T > rhs )
{
    return Vec2< T >(
        factor * rhs.x,
        factor * rhs.y );
}

template< typename T >
Vec3< T > operator*( typename Vec3< T >::coord_type factor, Vec3< T > rhs )
{
    return Vec3< T >(
        factor * rhs.x,
        factor * rhs.y,
        factor * rhs.z );
}

template< typename T >
Vec4< T > operator*( typename Vec4< T >::coord_type factor, Vec4< T > rhs )
{
    return Vec3< T >(
        factor * rhs.x,
        factor * rhs.y,
        factor * rhs.z,
        factor * rhs.w );
}

//! @brief The product of a vector and scalar
template< typename T >
Vec2< T > operator*( Vec2< T > lhs, typename Vec2< T >::coord_type factor )
{
    return factor * lhs;
}

template< typename T >
Vec3< T > operator*( Vec3< T > lhs, typename Vec3< T >::coord_type factor )
{
    return factor * lhs;
}

template< typename T >
Vec4< T > operator*( Vec4< T > lhs, typename Vec4< T >::coord_type factor )
{
    return factor * lhs;
}

//! @brief Equality (with tolerance) check
template< typename T >
bool operator==( Vec2< T > lhs, Vec2< T > rhs )
{
    return fabs( lhs.x - rhs.x ) < EPSILON &&
           fabs( lhs.y - rhs.y ) < EPSILON;
}

template< typename T >
bool operator==( Vec3< T > lhs, Vec3< T > rhs )
{
    return fabs( lhs.x - rhs.x ) < EPSILON &&
           fabs( lhs.y - rhs.y ) < EPSILON &&
           fabs( lhs.z - rhs.z ) < EPSILON;
}

template< typename T >
bool operator==( Vec4< T > lhs, Vec4< T > rhs )
{
    return fabs( lhs.x - rhs.x ) < EPSILON &&
           fabs( lhs.y - rhs.y ) < EPSILON &&
           fabs( lhs.z - rhs.z ) < EPSILON &&
           fabs( lhs.w - rhs.w ) < EPSILON;
}

//! @brief Non-equality (with tolerance) check
template< typename T >
bool operator!=( Vec2< T > lhs, Vec2< T > rhs )
{
    return !( lhs == rhs );
}

template< typename T >
bool operator!=( Vec3< T > lhs, Vec3< T > rhs )
{
    return !( lhs == rhs );
}

template< typename T >
bool operator!=( Vec4< T > lhs, Vec4< T > rhs )
{
    return !( lhs == rhs );
}

//! @brief Vector normalization
//! @f[ v = \frac{ v }{ ||v|| } @f]
//!
//! Produces a unit vector (or zero-vector + log warning)
template< typename T >
void normalize( Vec2< T > v )
{
    typename Vec2< T >::coord_type len = length( v );
    
    if ( len > EPSILON ) { // Avoid division-by-zero
        typename Vec2< T >::coord_type oneToLen = 1.0 / len;
        v *= oneToLen;
    } else {
        LOG_WARNING( "Attempting to normalize() zero vector" );
        v = Vec2< T >(
            static_cast< T >( 0 ),
            static_cast< T >( 0 ) );
    }
}

template< typename T >
void normalize( Vec3< T > v )
{
    typename Vec3< T >::coord_type len = length( v );
    
    if ( len > EPSILON ) { // Avoid division-by-zero
        typename Vec3< T >::coord_type oneToLen = 1.0 / len;
        v *= oneToLen;
    } else {
        LOG_WARNING( "Attempting to normalize() zero vector" );
        v = Vec3< T >(
            static_cast< T >( 0 ),
            static_cast< T >( 0 ),
            static_cast< T >( 0 ) );
    }
}

template< typename T >
void normalize( Vec4< T > v )
{
    typename Vec4< T >::coord_type len = length( v );
    
    if ( len > EPSILON ) { // Avoid division-by-zero
        typename Vec4< T >::coord_type oneToLen = 1.0 / len;
        v *= oneToLen;
    } else {
        LOG_WARNING( "Attempting to normalize() zero vector" );
        v = Vec4< T >(
            static_cast< T >( 0 ),
            static_cast< T >( 0 ),
            static_cast< T >( 0 ),
            static_cast< T >( 0 ) );
    }
}

//! @brief The length (in other words: norm, magnitude) of a vector
//!
//! @f[ ||v|| @f]
template< typename T >
typename Vec2< T >::coord_type length( Vec2< T > v )
{
	return sqrt(
        v.x * v.x +
        v.y * v.y );
}

template< typename T >
typename Vec3< T >::coord_type length( Vec3< T > v )
{
	return sqrt(
        v.x * v.x +
        v.y * v.y +
        v.z * v.z );
}

template< typename T >
typename Vec4< T >::coord_type length( Vec4< T > v )
{
	return sqrt(
        v.x * v.x +
        v.y * v.y +
        v.z * v.z +
        v.w * v.w );
}

//! @brief The squared length of a vector
//! @f[ ||v||^2 @f]
//!
//! Often handy. Computations are less costly than length() * length()
template< typename T >
typename Vec2< T >::coord_type lengthSq( Vec2< T > v )
{
	return (
        v.x * v.x +
        v.y * v.y );
}

template< typename T >
typename Vec3< T >::coord_type lengthSq( Vec3< T > v )
{
	return (
        v.x * v.x +
        v.y * v.y +
        v.z * v.z );
}

template< typename T >
typename Vec4< T >::coord_type lengthSq( Vec4< T > v )
{
	return (
        v.x * v.x +
        v.y * v.y +
        v.z * v.z +
        v.w + v.w );
}

//! @brief The dot product (also known as scalar product or inner product)
//     | ax |      | bx |
// a = | ay |, b = | by |
//     | az |      | bz |
// ab = ax*bx + ay*by + az*bz
template< typename T >
float dot( Vec2< T > lhs, Vec2< T > rhs )
{
	return (
        lhs.x * rhs.x +
        lhs.y * rhs.y );
}

template< typename T >
float dot( Vec3< T > lhs, Vec3< T > rhs )
{
	return (
        lhs.x * rhs.x +
        lhs.y * rhs.y +
        lhs.z * rhs.z );
}

template< typename T >
float dot( Vec4< T > lhs, Vec4< T > rhs )
{
	return (
        lhs.x * rhs.x +
        lhs.y * rhs.y +
        lhs.z * rhs.z +
        lhs.w * rhs.w );
}

//! @brief The cross product (also known as vector product)
//     | ax |      | bx |
// a = | ay |, b = | by |
//     | az |      | bz |
//       |  ay*bz - by*az |   | ay*bz - az*by |
// axb = | -ax*bz + bx*az | = | az*bx - ax*bz |
//       |  ax*by - bx*ay |   | ax*by - ay*bx |
template< typename T >
Vec3< T > cross( Vec3< T > lhs, Vec3< T > rhs )
{
	return Vec3< T >(
        lhs.y * rhs.z - lhs.z * rhs.y,
        lhs.z * rhs.x - lhs.x * rhs.z,
        lhs.x * rhs.y - lhs.y * rhs.x );
}

} // namespace math
